﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using System.Threading;

namespace TbaStarTest
{
    class TileMap
    {
        public int Columns { get; set; } // Nr of columns
        public int Rows { get; set; } // Nr of rows
        public Tile[,] Tiles { get; private set; }
        private int[,] tiles;
        private Tile start;
        private Tile goal;
        private List<Tile> opened = new List<Tile>();
        private List<Tile> closed = new List<Tile>();
        private List<Tile> bestList = new List<Tile>();

        // TBA*
        private List<Tile> pathNew = new List<Tile>();
        private List<Tile> pathToFollow = new List<Tile>();
        private bool solutionFound = false;
        private Tile currentLocation;
        private Tile lastLocation;       

        public TileMap()
        {
            // This twodimensional array represents the map of tiles.
            // 0 = nothing  1 = tile    (0, 0) = start  (9, 9) = end
            //tiles = new int[10, 10] { 
            ////        ---> columns -->
            //{ 0, 0, 0, 1, 1, 0, 0, 0, 0, 0 },
            //{ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }, // r
            //{ 1, 1, 1, 0, 1, 0, 0, 0, 0, 0 }, // o
            //{ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }, // w
            //{ 1, 0, 1, 1, 1, 0, 0, 0, 0, 0 }, // s
            //{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
            //{ 1, 1, 1, 0, 0, 0, 0, 0, 0, 0 }, 
            //{ 0, 0, 0, 1, 0, 0, 1, 0, 1, 0 }, 
            //{ 0, 0, 0, 0, 1, 0, 1, 0, 1, 0 }, 
            //{ 0, 0, 0, 0, 0, 0, 1, 0, 0, 0 }, 
            //};

            tiles = new int[20, 20] { 
            //        ---> columns -->
            { 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0 },
            { 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }, // r
            { 1, 1, 1, 0, 1, 0, 0, 0, 0, 0, 1, 1, 1, 0, 1, 0, 0, 0, 0, 0 }, // o
            { 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }, // w
            { 1, 0, 1, 1, 1, 0, 0, 0, 0, 0, 1, 0, 1, 1, 1, 0, 0, 0, 0, 0 }, // s
            { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
            { 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0 }, 
            { 0, 0, 0, 1, 0, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 1, 0, 1, 0 }, 
            { 0, 0, 0, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 1, 0 }, 
            { 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0 },
            { 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0 },
            { 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }, // r
            { 1, 1, 1, 0, 1, 0, 0, 0, 0, 0, 1, 1, 1, 0, 1, 0, 0, 0, 0, 0 }, // o
            { 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }, // w
            { 1, 0, 1, 1, 1, 0, 0, 0, 0, 0, 1, 0, 1, 1, 1, 0, 0, 0, 0, 0 }, // s
            { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
            { 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0 }, 
            { 0, 0, 0, 1, 0, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 1, 0, 1, 0 }, 
            { 0, 0, 0, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 1, 0 }, 
            { 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0 }
            };
            this.Rows = tiles.GetLength(0);
            this.Columns = tiles.GetLength(1);
            this.Tiles = new Tile[Rows, Columns];

            // Generate the tiles
            for (int row = 0; row < this.Rows; row++)
            {
                for (int column = 0; column < this.Columns; column++)
                {
                    this.Tiles[row, column] = new Tile(column, row, this.tiles[row, column], this);
                    Console.Write(this.tiles[row, column]);
                }
                Console.WriteLine();
            }

            GenerateAdjacencies();
        }

        /// <summary>
        /// Generate the adjacencies for all tiles.
        /// </summary>
        private void GenerateAdjacencies()
        {
            for (int i = 0; i < Rows; i++)
            {
                for (int j = 0; j < Columns; j++)
                {
                    Tiles[i, j].CalculateAdjacencies();
                }
            }
        }

        /// <summary>
        /// Initializes the A* algorithm, making it ready for TBA* use.
        /// <param name="start">The starting position.</param>
        /// <param name="goalLocation">The goal.</param>
        /// </summary>
        public void InitializeAstar(Point startLocation, Point goalLocation)
        { 
            // Reset all values so this method can be recalled for every new start and goal
            solutionFound = false;
            opened.Clear();
            closed.Clear();
            bestList.Clear();
            if (this.start != null)
                this.start.IsStart = false;
            if (this.goal != null)
                this.goal.IsEnd = false;

            this.start = Tiles[startLocation.X, startLocation.Y];
            this.goal = Tiles[goalLocation.X, goalLocation.Y];
            this.currentLocation = start;
            this.lastLocation = start;

            // Initialize the start and goal tile
            this.start.IsStart = true; // Start tile = Top left
            this.goal.IsEnd = true;

            // Adjacencies have been calculated during initialization. The adjacencies 
            // are all available tiles, NOT the walls.
            HashSet<Tile> adjacencies = start.Adjacencies;
            foreach (Tile adjacency in adjacencies) // Calculate adjacencies
            {
                adjacency.Parent = start;
                if (adjacency.IsStart == false)
                {
                    opened.Add(adjacency);
                }
            }
        }

        /// <summary>
        /// Find the best path using the A*-algorithm.
        /// </summary>
        /// <param name="expansionLimit">The expansion limit of the pathfinding process (maximum number of iterations).</param>
        /// <returns>Whether a solution has been found (meaning the goal has been reached) or not.</returns>
        public bool ContinueAstar(int expansionLimit)
        {
            Console.WriteLine("Calculating best path...");
            long startTime = DateTime.Now.Ticks;
            long endTime;
            long timeElapsed;

            // Clear the last path, making room for a new one.
            this.bestList.Clear();

            // Calculate best path to either the goal or the heuristic best 
            // tile if the expansion limit is reached
            //while (opened.Count > 0)
            int iterations = 0;
            while (opened.Count > 0 && iterations < expansionLimit)
            {
                iterations++;
                Tile best = findBestPassThrough();
                opened.Remove(best);
                closed.Add(best);
                if (best.IsEnd)
                {
                    Console.WriteLine("Found Goal");
                    PopulateBestList(goal);
                    endTime = DateTime.Now.Ticks;
                    timeElapsed = (endTime - startTime);
                    Console.WriteLine("Time elapsed: " + timeElapsed);
                    Print();
                    return true; // Goal found
                }
                else
                {
                    HashSet<Tile> neighbors = best.Adjacencies;
                    foreach (Tile neighbor in neighbors)
                    {
                        if (opened.Contains(neighbor))
                        {
                            Tile tmpSquare = new Tile(neighbor.X,
                                    neighbor.Y, neighbor.Type, this);
                            tmpSquare.Parent = best;
                            if (tmpSquare.getPassThrough(goal) >= neighbor
                                    .getPassThrough(goal))
                            {
                                continue;
                            }
                        }

                        if (closed.Contains(neighbor))
                        {
                            Tile tmpSquare = new Tile(neighbor.X,
                                    neighbor.Y, neighbor.Type, this);
                            tmpSquare.Parent = best;
                            if (tmpSquare.getPassThrough(goal) >= neighbor
                                    .getPassThrough(goal))
                            {
                                continue;
                            }
                        }

                        neighbor.Parent = best;

                        opened.Remove(neighbor);
                        closed.Remove(neighbor);
                        opened.Insert(0, neighbor);
                    }
                }
            }
            endTime = DateTime.Now.Ticks;
            timeElapsed = (endTime - startTime);
            Console.WriteLine("Time elapsed: " + timeElapsed);
            Console.WriteLine("No Path to goal");

            // The last item in closed list is always the best path, and the longest (most progress)
            PopulateBestList(closed[closed.Count - 1]);
            Print();
            return false; // Goal not found
        }

        /// <summary>
        /// Populate the bestList, backtracing from the goal tile or 
        /// the current 'best' tile if the goal hasn't been found yet.
        /// </summary>
        /// <param name="tile">The tile to backtrace from.</param>
        private void PopulateBestList(Tile tile)
        {
            bestList.Add(tile);
            //if (tile.Parent.IsStart == false)
            // If parent is not the starting location or the current location
            //if (tile.Parent.IsStart == false && !(tile.Parent.X == currentLocation.X && tile.Parent.Y == currentLocation.Y))
            //{
            //    PopulateBestList(tile.Parent);
            //}
            if (tile.IsStart == false && tile != currentLocation)
            {
                PopulateBestList(tile.Parent);
            }

            return;
        }

        /// <summary>
        /// Finds the path with the lowest cost from a tile as close as possible to 
        /// the start to the goal.
        /// </summary>
        /// <returns>Returns a tile closest to the the goal with the shortest possible path.</returns>
        private Tile findBestPassThrough()
        {
            // The closer a tile is to the goal, the greater the cost, because 
            // all the parent costs are added to this tile's cost as well.
            Tile best = null;
            foreach (Tile tile in opened)
            {
                if (best == null
                        || tile.getPassThrough(goal) < best.getPassThrough(goal))
                {
                    best = tile;
                }
            }

            return best;
        }

        /// <summary>
        /// Prints the map and the calculated path.
        /// </summary>
        private void Print()
        {
            for (int row = 0; row < this.Rows; row++)
            {
                for (int column = 0; column < this.Columns; column++)
                {
                    Tile tile = this.Tiles[row, column];
                    if (tile == currentLocation)
                        Console.Write("+");
                    else if (tile.IsStart)
                        Console.Write("S");
                    else if (tile.IsEnd)
                        Console.Write("E");
                    else if (this.bestList.Contains(tile))
                        Console.Write(".");
                    else
                        Console.Write(tile.Type);
                }
                Console.WriteLine();
            }
        }

        public void TbaStar()
        {
            while (currentLocation != goal)
            {
                if (!solutionFound)
                    solutionFound = ContinueAstar(1);

                pathNew = this.bestList;
                pathToFollow = pathNew;
                // doneTrace is skipped because we always trace back

                // Check if the last tile in the path equals the goal tile
                if (pathToFollow.Count > 0) 
                {
                    if (pathToFollow[0] == goal)
                        solutionFound = true;
                }

                if (pathToFollow.Contains(currentLocation) && pathToFollow.Count > 1)
                {   
                    // Move one step forward along the path.
                    // Pop the last element in the path, which is the front of the path.
                    pathToFollow.RemoveAt(pathToFollow.Count - 1);
                    currentLocation = pathToFollow[pathToFollow.Count - 1];
                }
                else // Path doens't contain the current location
                {
                    // Take one step back by backtracking
                    if (currentLocation != start)
                        currentLocation = currentLocation.Parent; // step back
                    else
                        currentLocation = lastLocation;
                }
                lastLocation = currentLocation;
                Console.WriteLine("CURRENT LOCATION: " + currentLocation.X + ", " + currentLocation.Y);

                Print();
                Thread.Sleep(300);

                if (currentLocation == goal)
                {
                    // Execute pathfinding for a new path
                    InitializeAstar(new Point(1, 1), new Point(1, 19));
                }
            }

            
        }
    }
}
